#ifndef URCONTROLOFFSETA_HPP
#define URCONTROLOFFSETA_HPP
#include "urcontroloffset.hpp"

class URControlOffsetA : public URControlOffset
{
public:
    static cv::Vec6d ComputeRT(const cv::Vec3d oxyFrom[3], const cv::Vec3d oxyTo[3])
    {
        cv::Vec3d fromBase[4];
        XYVectorToXYZUnitVector(oxyFrom,fromBase);
        cv::Vec3d toBase[4];
        XYVectorToXYZUnitVector(oxyTo,toBase);
        return ComputeTransform(fromBase, toBase);
    }

    static cv::Mat ChessBoard(cv::Size sz=cv::Size(6,9))
    {
        cv::Mat corner[3];
        corner[0]=(cv::Mat_<double>(sz.height,sz.width)<< -1.99363806e-02,   1.76182933e-04,   2.09168064e-02,
                   4.15087143e-02,   6.21965518e-02,   8.08186473e-02,
                   -1.84828143e-02,   3.26001436e-04,   2.08983972e-02,
                   4.15200212e-02,   6.20534564e-02,   8.25234934e-02,
                   -1.85162989e-02,   2.13316533e-03,   2.24510693e-02,
                   4.16421526e-02,   6.22513092e-02,   8.27355299e-02,
                   -1.84457919e-02,   2.16952735e-03,   2.28278778e-02,
                   4.30991799e-02,   6.27599134e-02,   8.30111944e-02,
                   -1.70758869e-02,   2.17416358e-03,   2.29009299e-02,
                   4.35378482e-02,   6.35724245e-02,   8.30416665e-02,
                   -1.66669832e-02,   2.85200705e-03,   2.34088879e-02,
                   4.36456097e-02,   6.44352157e-02,   8.35979894e-02,
                   -1.66699652e-02,   4.05365347e-03,   2.36774500e-02,
                   4.37873132e-02,   6.44514371e-02,   8.52724282e-02,
                   -1.62650112e-02,   4.06960244e-03,   2.48566227e-02,
                   4.57036303e-02,   6.46149597e-02,   8.54366609e-02,
                   -1.48530711e-02,   4.08234764e-03,   2.49724819e-02,
                   4.56244180e-02,   6.58724319e-02,   8.53933701e-02);

        corner[1]=(cv::Mat_<double>(sz.height,sz.width)<< -1.29259262e-01,  -1.29305363e-01,  -1.31318172e-01,
                   -1.31203461e-01,  -1.31446803e-01,  -1.31195210e-01,
                   -1.08923788e-01,  -1.10661071e-01,  -1.10592553e-01,
                   -1.10623407e-01,  -1.10476780e-01,  -1.12647729e-01,
                   -8.84348300e-02,  -9.03511971e-02,  -9.03517457e-02,
                   -9.02723709e-02,  -9.20525606e-02,  -9.18639236e-02,
                   -6.96468072e-02,  -6.96141946e-02,  -6.95425766e-02,
                   -7.14495803e-02,  -7.15506792e-02,  -7.14895388e-02,
                   -4.90158593e-02,  -4.90369023e-02,  -4.90384226e-02,
                   -5.08178750e-02,  -5.08102002e-02,  -5.08047379e-02,
                   -2.83096186e-02,  -2.83938573e-02,  -3.02202942e-02,
                   -3.02084142e-02,  -3.02339229e-02,  -3.05055790e-02,
                   -7.58581786e-03,  -9.45801744e-03,  -9.46376889e-03,
                   -9.47129473e-03,  -9.48355915e-03,  -1.13790569e-02,
                   1.12647551e-02,   1.12882903e-02,   1.12898801e-02,
                   1.13048599e-02,   9.41114430e-03,   9.41236277e-03,
                   3.21336075e-02,   3.21722530e-02,   3.22258382e-02,
                   3.01743146e-02,   3.02261657e-02,   3.02103828e-02);

        corner[2]=(cv::Mat_<double>(sz.height,sz.width)<< 6.83816730e-01,   6.83665780e-01,   6.84788123e-01,
                   6.84189937e-01,   6.85458899e-01,   6.83563746e-01,
                   6.85474746e-01,   6.84842483e-01,   6.84185429e-01,
                   6.84376309e-01,   6.83469197e-01,   6.85289799e-01,
                   6.86716594e-01,   6.86988764e-01,   6.86992935e-01,
                   6.86389406e-01,   6.85648388e-01,   6.84243336e-01,
                   6.86891124e-01,   6.86569482e-01,   6.85863150e-01,
                   6.86861721e-01,   6.87110288e-01,   6.86523149e-01,
                   6.87741412e-01,   6.88036666e-01,   6.88057997e-01,
                   6.86642054e-01,   6.86538353e-01,   6.86464548e-01,
                   6.88006837e-01,   6.88220270e-01,   6.88612280e-01,
                   6.88341578e-01,   6.88922830e-01,   6.88254955e-01,
                   6.88129935e-01,   6.87240348e-01,   6.87658261e-01,
                   6.88205105e-01,   6.89096265e-01,   6.89605881e-01,
                   6.88505793e-01,   6.89944272e-01,   6.90041441e-01,
                   6.90957009e-01,   6.90844601e-01,   6.90934045e-01,
                   6.91273682e-01,   6.92105043e-01,   6.93257794e-01,
                   6.89759459e-01,   6.90944732e-01,   6.90583949e-01);
        cv::Mat xyz;
        cv::merge(corner, 3, xyz);
        return xyz;
    }

    static cv::Mat ChessBoard2(cv::Size sz=cv::Size(6,9), double l=2e-2)
    {
        cv::Mat corner[3];

        std::vector<double> xv(sz.width,0);
        for(size_t i=1;i<xv.size();i++)
            xv[i]=xv[i-1]+l;
        corner[0]=cv::repeat(cv::Mat(xv).t(),sz.height,1);

//        std::cout<<"corner[0]="<<corner[0]<<"\n";

        std::vector<double> yv(sz.height,0);
        for(size_t i=1;i<yv.size();i++)
            yv[i]=yv[i-1]+l;
        corner[1]=cv::repeat(cv::Mat(yv),1,sz.width);

        corner[2]=cv::Mat::zeros(sz,CV_64FC1);

        cv::Mat xyz;
        cv::merge(corner, 3, xyz);
        return xyz;
    }

    static void dfff()
    {
        cv::Mat cb1=ChessBoard();
        cv::Mat cb2=ChessBoard2();

        cv::Matx33d R(0.999668, -0.025743, -0.001224,
                       -0.025772, -0.998781, -0.042097,
                       -0.000139, 0.042115, -0.999113);
        cv::Vec3d T(-0.033510, -0.020099, 0.688959);

        cv::Mat df(cb1.size(),cb1.type());

        for(size_t i=0;i<df.total();i++)
        {
            df.at<cv::Vec3d>(i)=R.t()*cb2.at<cv::Vec3d>(i)+T-cb1.at<cv::Vec3d>(i);
        }

        std::cout<<df<<"\n\n";
    }

//    any coordinate to base coordinate
    static cv::Vec6d Calibrate2()
    {
        cv::Mat xyz=ChessBoard();

        cv::Vec3d newoxy[3]={
            xyz.at<cv::Vec3d>(0,0),
            xyz.at<cv::Vec3d>(0,xyz.cols-1),
            xyz.at<cv::Vec3d>(xyz.rows-1,0)
        };

        cv::Vec6d finger2Head=cv::Vec6d(0,0,0.21,0,0,CV_PI/4);

        cv::Vec6d headPose[3]={
            cv::Vec6d(0.462104,-0.101315,0.239632,1.15068,2.87706,0.00874831),
            cv::Vec6d(0.367604,-0.109055,0.236241,1.14843,2.91417,0.0359343),
            cv::Vec6d(0.462698,0.0449286,0.236633,1.19043,2.86703,-0.0452107)
        };

        cv::Vec3d oldoxy[3]={
            HeadToBase(cv::Vec3d(0,0,0), ComposeTransform(finger2Head, headPose[0])),
            HeadToBase(cv::Vec3d(0,0,0), ComposeTransform(finger2Head, headPose[1])),
            HeadToBase(cv::Vec3d(0,0,0), ComposeTransform(finger2Head, headPose[2]))
        };

        cv::Vec6d new2old=ComputeRT(newoxy, oldoxy);
        return new2old;

    }


    static cv::Vec4d GetPlane(cv::Vec3d point[3])
    {
        cv::Vec3d normal=(point[2]-point[0]).cross(point[1]-point[0]);
        double d=-normal.ddot(point[0]);
        return cv::Vec4d(normal[0],normal[1],normal[2],d);
    }

    static cv::Vec4d MarkerPosToPlane(cv::Vec6d tf)
    {
        cv::Vec3d r(tf[3],tf[4],tf[5]);
        cv::Vec3d t(tf[0],tf[1],tf[2]);

        cv::Matx33d R;
        cv::Rodrigues(r,R);

        cv::Vec3d normal=R*cv::Vec3d(0,0,1);

        if(normal[2]<0)
            normal=-normal;

        double v=normal.ddot(t);

        return cv::Vec4d(normal[0],normal[1],normal[2],-v);
    }

    static cv::Vec3d LineEquation(cv::Vec2d point1, cv::Vec2d point2)
    {
        return cv::Vec3d(point1[0],point1[1],1).cross(cv::Vec3d(point2[0],point2[1],1));
    }

    static cv::Vec2d RectangleCornerToCenter(cv::Vec2d pixelCorner[4])
    {
        cv::Vec3d diagonal[2]={
            LineEquation(pixelCorner[0], pixelCorner[2]),
            LineEquation(pixelCorner[1], pixelCorner[3])
        };

        cv::Vec3d intersection=diagonal[0].cross(diagonal[1]);
        return cv::Vec2d(intersection[0]/intersection[2], intersection[1]/intersection[2]);
    }


public:
    URControlOffsetA()
    {
        baseOffset=Calibrate2();
    }

    void test()
    {
        cv::Mat cb=ChessBoard();

        for(size_t i=0;i<cb.total();i++)
        {
            cv::Vec3d p=cb.at<cv::Vec3d>(i);
            ZPointer(p[0],p[1],p[2]-0.005,i*0.1,0.1,0.2,true);
        }

    }
};

#endif // URCONTROLOFFSETA_HPP
